;;====================================================================
;;                         coordinates.lisp
;; This program provides :roll/geo/coordinates package.
;; This package gives <coordinates> and <cascaded-coordinates> class.
;; The most of this code refer to euslisp's coordinates system.
;;
;;                               written by R.Ueda(garaemon@gmail.net)
;;====================================================================

(defpackage :roll/geo/coordinates
  (:use #:common-lisp #:roll/math #:roll/util)
  (:documentation
   "roll/geo/coordinates package provides <coordinates> class.")
  (:export #:<coordinates>
           #:<cascaded-coords>
           #:pos-of #:rot-of
           #:newpos #:newrot #:newcoords
           #:translate #:locate
           #:rotate #:orient
           #:4x4-of
           #:transformation #:inverse-transformation
           #:x-axis #:y-axis #:z-axis
           #:make-coords
           #:coordinatesp
           #:+world-coords+))

(in-package :roll/geo/coordinates)

;; class: <coordinates>
;; 座標系クラス
;;   - pos: 座標
;;   - rot: 回転行列
(defclass* <coordinates>
    ()
  ((pos (make-float-vector 3))
   (rot (make-identity-matrix 3))))

;; function: coordinatesp
;; <coordinates>であればtを返す
(declaim (inline coordinatesp))
(defun coordinatesp (c)
  (typep c '<coordinates>))

;; method: newcoords
;; 第一引数のcoordsに第二引数のcのrotとposをセットする.
;; 第二引数のcのポインタをそのままセットすることに注意.
(defmethod newcoords ((coords <coordinates>) c)
  "set new coordinates"
  (setf (rot-of coords) (rot-of c)
        (pos-of c) (pos-of c))
  coords)

;; method: newpos
;; 第一引数coordsのposに第二引数のcのposをセットする.
;; 第二引数のcのポインタをそのままセットすることに注意.
(defmethod newpos ((coords <coordinates>) p)
  "set new position vector"
  (setf (pos-of coords) p)
  coords)

;; method: newrot
;; 第一引数coordsのrotに第二引数のcのrotをセットする.
;; 第二引数のcのポインタをそのままセットすることに注意.
(defmethod newrot ((coords <coordinates>) r)
  "set new rotation matrix"
  (setf (rot-of coords) r)
  coords)

;; method: parent-orientation
;; roll/geo/coordinates内だけで使われるメソッド. 外にはexportされない.
(defmethod parent-orientation ((coords <coordinates>) v wrt)
  (declare (type (simple-array single-float) v)
           (optimize (speed 3) (safety 0) (debug 0)))
  (cond ((or (eq wrt :local)            ;if wrt = :local
             (eq wrt coords))           ;v must be transform by the rot of coords
         (mv* coords v))
        ((or (eq wrt :world)            ;if wrt = :world
             (eq wrt +world-coords+))
         v)
        ((coordinatesp wrt)
         (mv* (worldrot-of wrt) v))
        (t (error "unknown wrt ~A" wrt))))

;; method: translate
;; <coordinates>を平行移動する.
;; wrt, -- :local or :world -- , で絶対座標系かローカル座標系かを指定する.
(defmethod translate ((coords <coordinates>) vec &key (wrt :world))
  "translate coordinates by relative position."
  (newpos coords (v+ (parent-orientation coords vec wrt) (pos-of coords)))
  coords)

;; method: translate
;; <coordinates>の位置をセットする.
;; wrt, -- :local or :world -- , で絶対座標系かローカル座標系かを指定する.
(defmethod locate ((coords <coordinates>) vec &key (wrt :world))
  "set coordinates position."
  (newpos coords (parent-orientation coords vec wrt))
  coords)

(defmethod rotate-with-matrix ((coords <coordinates>) mat wrt)
  "internal method"
  (cond 
    ((eq wrt :local)
     (m* (rot-of coords) mat (rot-of coords)))
    ((or (eq wrt :local) (eq wrt :parent))
     (m* mat (rot-of coords) (rot-of coords)))
    ((coordinatesp wrt)
     (let* ((r2 (worldrot-of wrt))
            (r2t (flip r2)))
       (m* mat r2t r2t)
       (m* r2 r2t r2t)
       (m* r2t (rot-of coords) (rot-of coords))))
    (t
     (error "unkown wrt"))
    )
  coords)

(defmethod orient-with-matrix ((coords <coordinates>) mat wrt)
  (cond ((eq wrt :local)
         (m* (rot-of coords) mat (rot-of coords)))
        ((eq wrt :world)
         (setf (rot-of coords) mat))
        ((coordinatesp wrt)
         (let ((r2 (worldrot-of wrt)))
           (m* r2 mat (rot-of coordinatesp))))
        (t (error "unkown wrt"))
        ))

(defmethod rotate ((coords <coordinates>) theta axis &key (wrt :local))
  "rotate coordinates by relative thetale."
  (with-slots (rot pos) coords
    (cond ((arrayp axis)
           (rotate-with-matrix coords (rotation-matrix theta axis) wrt))
          ((arrayp theta)
           (rotate-with-matrix coords theta wrt))
          (t
           (cond ((eq wrt :local)
                  (rotate-matrix rot theta axis nil rot))
                 ((eq wrt :world)
                  (rotate-matrix rot theta axis t rot))
                 ((coordinatesp wrt)
                  (rotate-with-matrix coords (rotation-matrix theta (axis->vec axis)) wrt))
                 (t (error "unkown wrt"))
                 ))
          )
    (newrot coords rot)
    coords))

(defmethod orient ((coords <coordinates>) theta axis &optional (wrt :local))
  (with-slots (pos rot) coords
    (cond ((arrayp axis)
           (orient-with-matrix coords (rotation-matrix theta axis) wrt))
          ((arrayp theta)
           (orient-with-matrix coords theta wrt))
          (t
           (cond ((eq wrt :local)
                  (rotate-matrix rot theta axis nil rot))
                 ((eq wrt :world)
                  (setf rot (rotation-matrix theta axis)))
                 ((coordinatesp wrt)
                  (orient-with-matrix coords (rotation-matrix theta axis) wrt))
                 (t (error "unkown wrt"))
                 ))
          )
    (newrot coords rot)
    coords))

;; method: x-axis
;; coordinatesのx軸を返す
(defmethod x-axis ((coords <coordinates>))
  (matrix-row (rot-of coords) 0))

;; method: y-axis
;; coordinatesのy軸を返す
(defmethod y-axis ((coords <coordinates>))
  (matrix-row (rot-of coords) 1))

;; method: z-axis
;; coordinatesのz軸を返す
(defmethod z-axis ((coords <coordinates>))
  (matrix-row (rot-of coords) 2))

;; method: copy-rot
;; coordinatesのrotをコピーする
(defmethod copy-rot ((coords <coordinates>))
  (let ((ret (make-float-matrix 3 3)))
    (copy-matrix (rot-of coords) ret)
    ret))

;; method: copy-pos
;; coordinatesのposをコピーする
(defmethod copy-pos ((coords <coordinates>))
  (let ((ret (make-float-vector 3)))
    (copy-vector (pos-of coords) ret)
    ret))

;; method: copy-coords
;; coordinatesをコピーする
(defmethod copy-coords ((coords <coordinates>))
  (make-coords :pos (copy-pos coords)
               :rot (copy-rot coords)))

(defmethod worldrot-of ((coords <coordinates>))
  (rot-of coords))

(defmethod worldpos-of ((coords <coordinates>))
  (pos-of coords))

(defmethod worldcoords-of ((coords <coordinates>))
  coords)

(defmethod copy-worldcoords ((coords <coordinates>))
  (copy-coords coords))

(defmethod parentcoords-of ((coords <coordinates>))
  +world-coords+)

(defmethod transform-coords ((c1 <coordinates>) (c2 <coordinates>) &optional (c3 (make-coords)))
  (if (eq c1 c3)
      (v+ (pos-of c1)
          (mv* (rot-of c1) (pos-of c2))
          (pos-of c3))
      (v+ (pos-of c1)
          (mv* (rot-of c1) (pos-of c2) (pos-of c3))
          (pos-of c3)))
  (m* (rot-of c1) (rot-of c2) (rot-of xhc3))
  c3)

;; from -> toへの変換を求める.
;; wrtによって:local/:worldを切り替える
(defmethod transformation ((from <coordinates>) (to <coordinates>) &optional (wrt :local))
  (let* ((world-to (worldcoords-of to))
         (world-from (worldcoords-of from))
         (inv-from (inverse-transformation world-from)))
    (cond ((eq wrt :local)
           (transform-coords inv world-to inv))
          ((eq wrt :world)
           (transform-coords world-to inv inv))
          ((coordinatesp wrt)
           (let ((wrt-world (worldcoords-of wrt)))
             (transform-coords world-to inv inv)
             (transform-coords (inverse-transformation wrt-world) inv inv)
             (transform-coords inv wrt-world inv)
             ))
          (t (error "unkown wrt")))
    inv-from))

(defmethod inverse-transformation ((c <coordinates>) &optional (result (make-coords)))
  (flip (rot-of c) (rot-of result))
  (transform (rot-of result) (pos-of c) (pos-of result))
  (scale -1.0 (pos-of result) (pos-of result))
  result)

(defmethod move-to ((coords <coordinates>) (c <coordinates>)
                    &optional (wrt :local))
  (cond ((eq wrt :local)
         (let ((cc (transform-coords coords c)))
           (newcoords coords cc)))
        ((eq wrt :world)
         (newcoords coords c))
        ((coordinatesp wrt)
         (let ((cc (transform-coords (worldcoords-of wrt) c)))
           (transform-coords (inverse-transformation (parentcoords-of coords)) cc cc)
           (newcoords coords cc)))
        (t
         (error "unkown wrt")
         )))

(defmethod 4x4-of ((coords <coordinates>) &optional (newval nil))
  (with-slots (rot pos) coords
    (if newval
        (progn
          (dotimes (i 3)
            (dotimes (j 3)
              (setf (aref rot i j) (aref newval i j)))
            (setf (aref pos i) (aref newval i 3))
            )
          coords)
      (progn
        (let ((ret (make-float-matrix 4 4)))
          (setf (aref ret 3 3) 1.0)
          (dotimes (i 3)
            (dotimes (j 3)
              (setf (aref ret i j) (aref rot i j)))
            (setf (aref ret i 3) (aref pos i))
            )
          ret)))))

(defmethod rpy-of ((coords <coordinates>) &optional rpy)
  (if rpy
      (progn
        (newrot coords (rpy-matrix (aref rpy 0)
                                   (aref rpy 1)
                                   (aref rpy 2)))
        coords)
      (progn
        (rpy-angle (rot-of coords))
        )))

(defun make-coords (&key (pos (make-float-vector 3))
                         (rot (make-identity-matrix 3))
                         (rpy nil)
                         (4x4 nil))
  (let ((c (make-instance '<coordinates> :pos pos :rot rot)))
    (cond (4x4
           (4x4-of c 4x4))
          (rpy
           (rpy-of c rpy))
          )
    c))

(eval-when (:compile-toplevel eval)
  (defconstant +world-coords+ (make-coords)))

;; class: <cascaded-coords>
;; 連結座標系クラス
(defclass* <cascaded-coords>
    (<coordinates>)
  ((parent nil)
   (changed nil)
   (descendants nil)
   (_worldcoords (make-coords))
   ))

;; method: changed
;; coordsのchangedフラグを立てる
(defmethod changed ((coords <cascaded-coords>))
  (setf (changed-of coords) t)
  )

;; method: newcoords
;; <coordinates>クラスのnewcoordsを呼び出して,
;; その後にchangedフラグを立てる
(defmethod newcoords ((coords <cascaded-coords>) c)
  (call-next-method coords c)
  (changed coords)
  )

;; method: newpos
;; <coordinates>クラスのnewposを呼び出して,
;; その後にchangedフラグを立てる
(defmethod newpos ((coords <cascaded-coords>) p)
  (call-next-method coords p)
  (changed coords)
  )

;; method: newrot
;; <coordinates>クラスのnewrotを呼び出して,
;; その後にchangedフラグを立てる
(defmethod newrot ((coords <cascaded-coords>) r)
  (call-next-method coords r)
  (changed coords)
  )

(defmethod worldcoords-of ((coords <cascaded-coords>))
  (when changed
    ;; changedフラグが立っていた場合は再計算が必要
    (if parent
        ;; 何か他の座標にassocされていた場合は
        ;; 上の座標から自分の世界座標を再計算しなくてはいけない
        (transform-coords (worldcoords-of parent) coords (_worldcoords-of coords))
        ;; assocされていないときは...
        (copy-coords coords (_worldcoords-of coords))
        )
    (setf (changed-of coords) nil)
    )
  (_worldcoords-of coords))

(defmethod copy-worldcoords ((coords <cascaded-coords>))
  (copy-coords (_worldcoords-of coords)))

;; (load "/Users/garaemon/prog/URL/src/lisp/geo/coordinates.lisp")
;; (use-package :roll/geo/coordinates)
